package org.robotfish.geometry;

import java.util.ArrayList;
import java.util.Collections;
import org.ejml.simple.SimpleMatrix;

/**
 * List of transformations, the results of which are interpolated to find result.
 * It is the super class of all transformation forks.
 * Transformations form a tree, where siblings are listed.
 * The generic is the class of the siblings transformations.
 * @author Mart
 */
public abstract class TransformationFork<T extends Transformation> extends Transformation {
	
	ArrayList<T> segments = new ArrayList<T>();
	
	public void add(T segmenet) {
		segments.add(segmenet);
	}
	
	@Override
	public SimpleMatrix getRay() {
		Transformation A = segments.get(0);
		Transformation B = segments.get(segments.size()-1);
		return A.getRay().plus(B.getRay()).scale(0.5);
	}

	/**
	 * Finds index of first interval before v, unless v is passed the last interval, in which case the index of the interval preceding the last is returned.
	 * It uses instance of class Transformation to find index 
	 * @param list
	 * @param v
	 * @return
	 */
	private int find(double v) {
//		ArrayList<? extends Transformation> list = getSegments();
		int index = Collections.binarySearch(segments, new Start(v));
		if (index<0) // index = (-(insertion point) - 1) 
			index = -index-1; // index == insertion point
		// Kuna soovime interpoleerida, siis viimase elemndi indeksit ei anna
		if (index >= (segments.size()-1))
				index = (segments.size()-2);
		return index;
	}
	
	@Override
	public SimpleMatrix transform(SimpleMatrix point) {
		Transformation X0, X1;
		double x = point.get(getCoordinate()-1);
		int index = find(x);
		X0 = segments.get(index);
		X1 = segments.get(index+1);
		double m0 = (X1.coordinateValue-X0.coordinateValue);
		if (m0 != 0) // pole tegemist on kahe täpselt sama Transformatsiooniga 
			m0 = (X1.coordinateValue-x)/m0;
		return X0.interpolate(point, X1, m0); 
	}

}

/**
 * Class to hold start of the interval of reference coordinates.
 * It is used only for searching among transformations.
 * @author Mart Anton
 */
class Start extends Transformation {

	public Start(double coordinateValue) {
		this.coordinateValue = coordinateValue;
	}

	@Override
	public int getCoordinate() {
		throw new RuntimeException("Not implemented!");
	}

	@Override
	public SimpleMatrix getRay() {
		throw new RuntimeException("Not implemented!");
	}

	@Override
	public SimpleMatrix transform(SimpleMatrix point) {
		throw new RuntimeException("Not implemented!");
	}
}
